#define F_CPU 20000000UL  // 20 MHz CPU clock
#define STEPS_PER_REV 48
#define NUM_OF_HOLES 16
#define	DELAY_TIME 10
#define SPIN_RPM 0x13 	//	This value is calculated from 20M/1024/6.4/RPM

#include <stdio.h>
#include <stdlib.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#include "serial.h"

// Global variables
Serialt serial;									//serial port object
uint8_t charIn; 								//USART incoming character
char str[150];						//default string
int mode = 0;
//mode 0: awaiting command
//mode 1: stepping mode
//mode 2: spin mode
int ramp = 0;
int spin_dir = 0;
int microstep = 0;
int step_pos = 0;								//0 is home position
int	secs_left = DELAY_TIME;
int hole_number = 0;

#define _RESETTIMER \
	TCNT1L = 0; \
	TCNT1H = 0;
	
#define _SERIALWRITE(strlen) \
	for(int i=0; i<strlen; i++) \
		serial_write(&serial, str[i]);

#define _CLS \
	sprintf(str,"\r                             "); \
	_SERIALWRITE(30);

#define _DISPLAYCMD \
	_CLS; \
	sprintf(str, "\rCommand:"); \
	_SERIALWRITE(9);

#define _DISPLAYMENU \
	sprintf(str, "\n\rBioSpinner Menu\r\n\ns - Set home\n\rg - Go home\n\r[ - Adjust -\n\r] - Adjust +\n\ra - Abort\n\rt - Step\n\rp - Spin\n\ri - Info\r\n\n"); \
	_SERIALWRITE(118);

#define _LEDON \
	PORTC |= _BV(PC0); // Toggle on LED pin

#define _LEDOFF \
	PORTC &= ~_BV(PC0);

void step(int n_steps,int direction)
{
	if(direction==1)
	{
		PORTC |= _BV(PC4);	// initialize stepper dir pin to CCW
		step_pos = (step_pos+STEPS_PER_REV-n_steps) % STEPS_PER_REV;
	}
	else
	{
		PORTC &= ~_BV(PC4); // initialize stepper dir pin to CW
		step_pos = (step_pos+n_steps) % STEPS_PER_REV;
	}

	for(int j=0; j<n_steps; j++)
	{
		for(int i=0; i<8; i++)
		{
			PORTC |= _BV(PC5); // Pull high stepper step pin
			_delay_ms(5);
			PORTC &= ~_BV(PC5); // Lower stepper step pin
			_delay_ms(5);
		}
	}
}

ISR (TIMER1_COMPA_vect) // Timer 1 is a 16-bit timer
{
	cli(); // Temporarily disable all interrupts
	_RESETTIMER;

	switch(mode)
	{
		case 0:
			//	Await command
			break;
		case 1:
			//	Stepping sequence
			_LEDON;
			_CLS;
			sprintf(str, "\rHole(%2.0d): %2.0ds",hole_number,--secs_left);
			_SERIALWRITE(14);
			if(secs_left <= 0)
			{
				step(3,0);
				hole_number++;
				secs_left = DELAY_TIME;
				if(hole_number >= NUM_OF_HOLES)
				{
					mode=0;
					hole_number=0;
					_DISPLAYCMD;
				}
			}
			_LEDOFF;
			break;
		case 2:
			//	Spinning sequence
			
			PORTC &= ~_BV(PC4);	//Direction CW
			ramp--;
			if(spin_dir == 0 && ramp <= 0)
			{
				if(OCR1AL > 0x80)
				{
					OCR1AL = OCR1AL - 70;
					ramp=1;
				}
				else if(OCR1AL > 0x30)
				{
					OCR1AL = OCR1AL - 1;
					ramp = 3;
				}
				else if(OCR1AL > SPIN_RPM)
				{
					OCR1AL = OCR1AL - 1;
					ramp = 20;
				}
			}
			else if(spin_dir == 1 && ramp <= 0)
			{	//	spin down
				if(OCR1AL < 0x30)
				{
					OCR1AL = OCR1AL + 1;
					ramp = 20;
				}
				else if(OCR1AL < 0x80)
				{
					OCR1AL = OCR1AL + 1;
					ramp = 2;
				}
				else if(OCR1AL < 0xA0 && microstep == 0)
				{
					mode = 0;
					ramp = 0;
					OCR1AH = 0x4C;
					OCR1AL = 0x4B;
					_CLS;
					sprintf(str, "\rAbort");
					_SERIALWRITE(6);
					_delay_ms(1500);
					_DISPLAYCMD;
					return;
				}
			}
			PORTC |= _BV(PC5); // Pull high stepper step pin
			_delay_us(100);
			PORTC &= ~_BV(PC5); // Lower stepper step pin
			microstep++;
			if(microstep >= 8)
			{
				step_pos = (step_pos+1) % STEPS_PER_REV;
				microstep = 0;
			}

			break;
		default:
			//	Throw an error maybe...
			break;
	}

	sei(); // Re-enable interrupts
}

void commMonitor(void)
{
	// Deal with incoming serial bytes here
	if (serial_read(&serial,&charIn)) //if there's data read it
	{
		serial_write(&serial, charIn); //echo it back out for now...
		switch (charIn)
		{
			case '[':
				// Backward step
				mode = 0;
				OCR1AH = 0x4C;
				OCR1AL = 0x4B;
				step(1,1);
				_CLS;
				_DISPLAYCMD;
				break;
			case ']':
				// Forward step
				mode = 0;
				OCR1AH = 0x4C;
				OCR1AL = 0x4B;
				step(1,0);
				_CLS;
				_DISPLAYCMD;
				break;
			case 'p':
				// Spin up
				_CLS;
				sprintf(str, "\rSpinning (a - abort)");
				_SERIALWRITE(21);
				mode = 2;
				spin_dir = 0;
				microstep = 0;
				ramp = 0;
				OCR1AH = 0x00;
				OCR1AL = 0xFF;
				break;
			case 'i':
				//	Info
				mode = 0;
				OCR1AH = 0x4C;
				OCR1AL = 0x4B;
				_CLS;
				sprintf(str, "\rWritten by Ritchie Lee");
				_SERIALWRITE(23);
				_delay_ms(1500);
				_DISPLAYCMD;
				break;
			case 't':
				// Start stepping from home
				hole_number=0;
				secs_left = DELAY_TIME;
				_CLS;
				sprintf(str, "\rStepping");
				_SERIALWRITE(9);
				_delay_ms(1000);
				mode = 1;
				OCR1AH = 0x4C;
				OCR1AL = 0x4B;
				break;
			case 'a':
				// Abort
				if(mode == 2)
				{
					spin_dir = 1;	// spin down	
				}
				else
				{
					mode = 0;
					ramp = 0;
					OCR1AH = 0x4C;
					OCR1AL = 0x4B;
					_CLS;
					sprintf(str, "\rAbort");
					_SERIALWRITE(6);
					_delay_ms(1500);
					_DISPLAYCMD;
				}
				break;
			case 's':
				// Set home
				mode = 0;
				OCR1AH = 0x4C;
				OCR1AL = 0x4B;
				step_pos = 0;
				_CLS;
				sprintf(str, "\rSetting Home.");
				_SERIALWRITE(14);
				_delay_ms(1500);
				_DISPLAYCMD;
				break;
			case 'g':
				// Go home
				mode = 0;
				OCR1AH = 0x4C;
				OCR1AL = 0x4B;
				sprintf(str, "\rGoing Home.");
				_SERIALWRITE(13);
				step(step_pos,1);
				_DISPLAYCMD;
				break;
			case 'd':
				// Debug info
				mode = 0;
				OCR1AH = 0x4C;
				OCR1AL = 0x4B;
				_CLS;
				sprintf(str, "\rStep Position: %2.0d",step_pos);
				_SERIALWRITE(18);
				_delay_ms(1500);
				_DISPLAYCMD;
				break;
			default:
				_DISPLAYCMD;
				break;
		}

	}

	return;
}


int main(void) 
{
	
	// Initialization

	// 	Set up serial port
	serial_init(&serial);
	
	//	Set up IO pins
	DDRC |= _BV(PC0); // set led pin to output direction
	PORTC |= _BV(PC0); // led on, pin high

	DDRC |= _BV(PC4); // set stepper dir pin to output direction
	PORTC &= ~_BV(PC4); // initialize stepper dir pin to off
	DDRC |= _BV(PC5); // set stepper step pin to output direction
	PORTC |= _BV(PC5); // initialize stepper step pin to off
		
	//	Set up timer
	TCCR1A |= _BV(7) | _BV(6); // Output compare mode, CTC mode, prescaler 1024, set flag to 1
	TCCR1B |= _BV(3) | _BV(2) | _BV(0); 
	OCR1AH = 0x4C; // Interrupt each time timer1 gets to this value (which ticks at 20.0Mhz / 1024 = 19.5 kHz)
	OCR1AL = 0x4B; // lower half of previous line. 0x4C4B is roughly 1 second per interrupt
					// This value is calculated by 
	
	TCNT1H = 0; // Set timer top
	TCNT1L = 0; // Set timer bottom
	
	TIMSK1 |= _BV(1); // Enable timer interrupt

	sei(); // Globally enable interrupts

	_DISPLAYMENU;
	_DISPLAYCMD;
    while (1)                       /* loop forever */
    {
		commMonitor();	// Monitor serial port for incoming bytes
    }
	
	
	return 0;
}
